#!/usr/bin/python
# -*- coding: UTF-8 -*-
import math
import time
import numpy as np
import bisect
import matplotlib.pyplot as plt
import numpy as np





############################################
# 原始数据
data_distance = np.loadtxt(
    '/home/zy/Desktop/CCPP_backup/boustrophedo/CCPP_path_points_origin.csv')
fig, ax_distance = plt.subplots(1, 1)
data_distance_x = 1*data_distance[:, 0]
data_distance_y = 1*data_distance[:, 1]
ax_distance.plot(data_distance_x[0], data_distance_y[0], 'ro')
ax_distance.plot(data_distance_x, data_distance_y, 'b.')
ax_distance.plot(data_distance_x, data_distance_y, 'b.')
ax_distance.set_title('CCPP_path_points_distance trajectory')
ax_distance.set_aspect("equal", "box")


# # hybrid_path_debug_transform
# data_distance = np.loadtxt(
#     '/home/zy/Desktop/CCPP_backup/boustrophedo/hybrid_path_debug_transform.csv')
# fig, ax_distance = plt.subplots(1, 1)
# data_distance_x = 1*data_distance[:, 0]
# data_distance_y = 1*data_distance[:, 1]
# ax_distance.plot(data_distance_x[0], data_distance_y[0], 'ro')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.set_title('hybrid_path_debug_transform')
# ax_distance.set_aspect("equal", "box")


# data_distance = np.loadtxt(
#     '/home/zy/Desktop/CCPP_backup/boustrophedo/fov_poses.csv')
# fig, ax_distance = plt.subplots(1, 1)
# data_distance_x = 1*data_distance[:, 0]
# data_distance_y = 1*data_distance[:, 1]
# ax_distance.plot(data_distance_x[0], data_distance_y[0], 'ro')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.set_title('fov_poses')
# ax_distance.set_aspect("equal", "box")




# data_distance = np.loadtxt(
#     '/home/zy/Desktop/CCPP_backup/boustrophedo/fov_poses_hybrid.csv')
# fig, ax_distance = plt.subplots(1, 1)
# data_distance_x = 1*data_distance[:, 0]
# data_distance_y = 1*data_distance[:, 1]
# ax_distance.plot(data_distance_x[0], data_distance_y[0], 'ro')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.plot(data_distance_x, data_distance_y, 'b.')
# ax_distance.set_title('fov_poses_hybrid')
# ax_distance.set_aspect("equal", "box")




# 动态显示路径
# for i in range(len(data_distance_x)):
#     plt.plot(data_distance_x[i],data_distance_y[i],'.')     
    
#     plt.pause(0.001)
#     plt.draw()#注意此函数需要调用


plt.show()






